/*
    This source file is part of GeNa project.
    Copyright © GeNa project 2008
    For the latest info, see http://code.google.com/p/genaproject/ or  http://lumpyproject.forums-rpg.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Foobar is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Foobar.  If not, see <http://www.gnu.org/licenses/>. */

#ifndef DISTANCE_H_INCLUDED
#define DISTANCE_H_INCLUDED

#ifndef WX_PCH_H_INCLUDED
#include "wx_pch.h"
#endif
#include <cmath>
#include "math/vector.h"
using namespace math;

#define EPSILON 4*FLT_EPSILON

inline float sqDistPointToLine(vector3f p, vector3f v, vector3f a) //Return squared-distance of a to the line p + t*v, v must be NORMALIZED
{
    vector3f pa = p - a;
    int d = pa|v;
    return fabs(pa.squaredLength() - 2*d*d + v.squaredLength());
}

inline float sqDistLineToLine( vector3f p1, vector3f v1, vector3f p2, vector3f v2) //Return squared-distance between lines p1 + t*v1 and p2 + t*v2, v1 and v2 must be NORMALIZED
{
    vector3f n = v1^v2;
    float sqn = n.squaredLength();

    if( sqn < EPSILON) //Lines are parallel
        return (p1-p2).squaredLength();

    float d = (p1-p2)|n;
    return d*d / sqn;
}

inline float sqDistLineToSegment( vector3f p, vector3f v, vector3f a, vector3f b) //Return squared-distance between line p1 + t*v1 and segment [a,b]
{
    vector3f ab = a - b;

    if( ab.squaredLength() < EPSILON) //Check if the segment degenerate to a point
        return sqDistPointToLine(p,v,a);

    float abv = ab|v;
    float denom = v.squaredLength()*ab.squaredLength() - abv*abv;

    if( fabs(denom) < EPSILON) //Parallel line and segment
        return sqDistPointToLine(p,v,a);

    vector3f ap = a - p;
    float tdenom = v.squaredLength()*(ap|ab) - (ap|v) * (ab|v);

    if( tdenom <= 0)
        return sqDistPointToLine(p,v,a);
    if( tdenom >= denom)
        return sqDistPointToLine(p,v,b);

    //Same as in sqDistLineToLine
    vector3f n = v^ab;
    float sqn = n.squaredLength();

    float d = ap|n;
    return d*d / sqn;
}

#endif // DISTANCE_H_INCLUDED
